#ifndef _PLANNING_VISUALIZATION_H_
#define _PLANNING_VISUALIZATION_H_

#include <Eigen/Eigen>
#include <algorithm>
#include <iostream>
#include <ros/ros.h>
#include <vector>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
// #include <stdlib.h>

using std::vector;
namespace ego_planner {
class Visualization
{
private:
  ros::NodeHandle node;

  ros::Publisher goal_point_pub;
  ros::Publisher global_list_pub;
  ros::Publisher init_list_pub;
  ros::Publisher optimal_list_pub;
  ros::Publisher a_star_list_pub;
  ros::Publisher guide_vector_pub;
  ros::Publisher intermediate_state_pub;

  // DEBUG
  ros::Publisher debug_poly_pub;
  ros::Publisher debug_inter_pub;
  ros::Publisher debug_yaw_arrow_pub;

public:
  Visualization(/* args */) {}
  ~Visualization() {}
  Visualization(ros::NodeHandle &nh);

  typedef std::shared_ptr<Visualization> Ptr;

  void displaySphereList(ros::Publisher &pub, const vector<Eigen::Vector3d> &list, double scale,
                         Eigen::Vector4d color, int id);
  void displayLine(ros::Publisher &pub,
                   const vector<Eigen::Vector3d> &list,
                   double scale,
                   Eigen::Vector4d color,
                   int id);
  void displaySphereAndLineList(ros::Publisher &pub, const vector<Eigen::Vector3d> &list,
                                double scale, Eigen::Vector4d color, int id);
  void generatePathDisplayArray(visualization_msgs::MarkerArray &array,
                                const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id);
  void generateArrowDisplayArray(visualization_msgs::MarkerArray &array,
                                 const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id);
  void displayGoalPoint(Eigen::Vector3d goal_point, Eigen::Vector4d color, const double scale, int id);
  void displayGlobalPathList(vector<Eigen::Vector3d> global_pts, const double scale, int id);
  void displayInitPathList(vector<Eigen::Vector3d> init_pts, const double scale, int id);
  void displayMultiInitPathList(vector<vector<Eigen::Vector3d>> init_trajs, const double scale);
  void displayOptimalList(Eigen::MatrixXd optimal_pts, int id);
  void displayAStarList(std::vector<std::vector<Eigen::Vector3d>> a_star_paths, int id);
  void displayArrowList(ros::Publisher &pub, const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id);

  void displayPolygon(const vector<Eigen::Vector2d> &list, double scale, int id);
  void pubPolygon(const vector<Eigen::Vector2d> &list, double scale,
                  Eigen::Vector4d color, int id);
  void displayInterPoint(const vector<Eigen::Vector2d> &list, double scale, Eigen::Vector4d color, int id);
  void displayYawArrowList(const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id);
};
} // namespace ego_planner
#endif